//=============================================================================
//=============================================================================
// Filename: 
//=============================================================================
//=============================================================================
//=============================================================================
//
//=============================================================================

//----------------------------------------------------------------------------

#include <p18F4520.h>
#include <delay.h>
#include "ieee_robot.h"
#include "motor.h"
#include "servo.h"


#pragma config OSC = EC //this sets the oscillator to external clock on
//OSC1 pin (pin 13). The PICDEM 2 plus board
//uses a 4 MHz external clock, other configs
//should use different setting

unsigned short int servo_position = SERVO_STRAIGHT; 






/*-------------------Local Functions----------------------------------------------*/

void delay_ms(unsigned char delay) {
    #undef MULT //undef any previous defines of MULT

#if TCYCNT_4_MS >= 10000
    #if TCYCNT_4_MS > 10000
        #define MULT TCYCNT_4_MS / 10000
    #else
        #define MULT 1
    #endif
        Delay10KTCYx(MULT*delay);
#elif TCYCNT_4_MS >= 1000
    #if TCYCNT_4_MS > 1000
        #define MULT TCYCNT_4_MS / 1000
    #else
        #define MULT 1
    #endif
        Delay1KTCYx(MULT*delay);
#elif TCYCNT_4_MS >= 100
    #if TCYCNT_4_MS > 100
        #define MULT TCYCNT_4_MS / 100
    #else
        #define MULT 1
    #endif
        Delay100TCYx(MULT*delay);
#elif TCYCNT_4_MS >= 10
    #if TCYCNT_4_MS > 10
        #define MULT TCYCNT_4_MS / 10
    #else
        #define MULT 1
    #endif
        Delay10TCYx(MULT*delay);
#elif TCYCNT_4_MS < 10
        unsigned char i;
        for (i = 0; i < TCYCNT_4_MS;i++) {
            _asm
                nop
            _endasm
        }
        return;
#else
    #warning "Something went wrong in delay_ms, IEEE_Robot_Main.c" 
#endif
}

void test() {
    TRISB = 0;

    while(1) {
        PORTB = 0xFF;
        delay_ms(1);
        PORTB = 0x00;
    }
}
/*-------------------Timer 3 operation functions and variables--------------------*/


//----------------------------------------------------------------------------
// Main routine
void main (void) {
    //Flags.Byte = 0;
    int i = 0;
/*--------Timer 0 and Timer 1 setup and interrupt configuration--------------------*/
    INTCON = 0x20;                //disable global and enable TMR0 interrupt
                                  //while clearing TMR0IF flag bit
    RCONbits.IPEN = 1;            //enable interrupt priority levels
    INTCON2 = 0x84;               //TMR0 high priority                            

    IPR1 = IPR1 | 0x01;           //set Timer 1 interrupts high priority
    PIR1 = PIR1 & (~0x01);         //clear TMR1F interrupt flag
    PIE1 = PIE1 & (~0x01);        //disable Timer 1 interrupts

    write_timer0(TMR0_VALUE);     //write to Timer 0 the value determine by 
                                  //experimentation to provide 50 Hz interrupts
    T0CON = 0x88;  //set up timer0 - prescaler 1:1, with 16
                   //bit selected

    T1CON = 0x91;  //setup TICON for TMR1 to use Fosc/4 with 1:2 prescaler
                   //with 16 bit rights and read of TMR1H:TMR1:L


    servo_position = SERVO_LEFT_45;                
    INTCONbits.GIEH = 1;          //enable interrupts

    //init PORT B
    PORTB = 0;
    TRISB = 0x00;   //port B is all output

    //init PORT A
    PORTA = 0;
    TRISA = 0xFF; //port a all inputs

    //note 100 = 50% duty cycle
    init_ccp1();
    set_duty_ccp1(100);
    start_ccp1();

//set port b as input
    
    while (1) {
        if (PORTAbits.RA4 == 0) {
            set_duty_ccp1(i++);
        }

    }
}




